/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-24 16:29:03
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-11-26 03:10:34
 * @Description: ...
 * @Reference: ...
 */
#include "lci_slam/vilens/camera/camera_processor.h"

namespace vilens {

cameraDatabase::cameraDatabase() {}

cameraProcessor::cameraProcessor() {
    // allocate memory
    cameraDatabasePtr.reset(new cameraDatabase());

    featureDatabase.reset(new ov_core::FeatureDatabase());
    featureTrackerPtr.reset(new ov_core::TrackKLT(
        trackCameras, maxNumPoints, 0, isStereo, methodHist, fastThreshold,
        minPixDist, gridX, gridY));
}

cameraProcessor::~cameraProcessor() {}

void cameraProcessor::setConfig() {}

bool cameraProcessor::judgeKeyframe() {
    if (1) {
    }

    return true;
}

bool cameraProcessor::processMono(double timestamp, const cv::Mat &img,
                                  int cam_id, const cv::Mat &mask) {
    ov_core::CameraData message;

    message.sensor_ids.resize(1);
    message.images.resize(1);
    message.masks.resize(1);
    message.sensor_ids[0] = cam_id;
    message.timestamp = timestamp;

    message.images[0] = img;
    message.masks[0] = mask.empty() ? createMask(cam_id) : mask;

    featureTrackerPtr->feed_new_camera(message);
    featureDatabase->append_new_measurements(
        featureTrackerPtr->get_feature_database());

    return true;
}

bool cameraProcessor::processStereo(double timestamp, const cv::Mat &img0,
                                    const cv::Mat &img1, int cam_id0,
                                    int cam_id1, const cv::Mat &mask0,
                                    const cv::Mat &mask1) {
    ov_core::CameraData message;

    message.sensor_ids.resize(2);
    message.images.resize(2);
    message.masks.resize(2);
    message.sensor_ids[0] = cam_id0;
    message.sensor_ids[1] = cam_id1;
    message.timestamp = timestamp;

    message.images[0] = img0;
    message.images[1] = img1;
    message.masks[0] = mask0.empty() ? createMask(cam_id0) : mask0;
    message.masks[1] = mask1.empty() ? createMask(cam_id1) : mask1;

    featureTrackerPtr->feed_new_camera(message);
    featureDatabase->append_new_measurements(
        featureTrackerPtr->get_feature_database());

    return true;
}

cv::Mat cameraProcessor::createMask(const size_t &cam_id) {
    return cv::Mat(trackCameras[cam_id]->camera_ptr->imageHeight(),
                   trackCameras[cam_id]->camera_ptr->imageWidth(), CV_8UC1,
                   cv::Scalar(255));
}

}  // namespace vilens